Least squares tracking on the Euclidean group

A large class of problems in robotics, e.g., trajectory tracking with obstacle avoidance, compliant motion control, and complex assembly, can be formulated as a least-squares tracking problem on the Euclidean group subject to constraints on the state and/or control. In this note we develop an optimal control framework for this general class of problems, and derive analytic solutions for the local and global versions of the general optimal control problem. Our formalism can be viewed in some sense as an extension to the Euclidean group of the linear quadratic regulator (LQR) subject to state equality constraints. Examples from force-guided complex assembly and tracking with obstacle avoidance are given.